#include "frame_control.h"
#include "frame.h"
#include <QObject>

FrameControl::FrameControl(QObject *parent) : Frame(8, 0x000031, parent) {}

QByteArray FrameControl::EncodeToBytes() const
{
    QByteArray array = QByteArray(Size(), 0);
    // 1
    array[0] &= 0b0011'1111;
    array[0] |= static_cast<unsigned char>(wheel_leg_selection);
    // 2
    array[0] &= 0b1101'1111;
    array[0] |= static_cast<unsigned char>(top_rocker);
    // 3
    array[0] &= 0b1110'1111;
    array[0] |= static_cast<unsigned char>(bottom_rocker);
    // 4
    array[0] &= 0b1111'0111;
    array[0] |= static_cast<unsigned char>(left_rocker);
    // 5
    array[0] &= 0b1111'1011;
    array[0] |= static_cast<unsigned char>(right_rocker);
    // 8
    array[1] &= 0b0111'1111;
    array[1] |= static_cast<unsigned char>(rise);
    // 9
    array[1] &= 0b1011'1111;
    array[1] |= static_cast<unsigned char>(decline);
    // 10
    array[1] &= 0b1101'1111;
    array[1] |= static_cast<unsigned char>(left_rise);
    // 11
    array[1] &= 0b1110'1111;
    array[1] |= static_cast<unsigned char>(left_decline);
    // 12
    array[1] &= 0b1111'0111;
    array[1] |= static_cast<unsigned char>(right_rise);
    // 13
    array[1] &= 0b1111'1011;
    array[1] |= static_cast<unsigned char>(right_decline);
    // 14
    array[1] &= 0b1111'1101;
    array[1] |= static_cast<unsigned char>(front_axle_rise);
    // 15
    array[1] &= 0b1111'1110;
    array[1] |= static_cast<unsigned char>(front_axle_decline);
    // 16
    array[2] &= 0b0111'1111;
    array[2] |= static_cast<unsigned char>(rear_axle_rise);
    // 17
    array[2] &= 0b1011'1111;
    array[2] |= static_cast<unsigned char>(rear_axle_decline);
    // 18
    array[2] &= 0b1101'1111;
    array[2] |= static_cast<unsigned char>(reset);
    // 19
    array[2] &= 0b1110'1111;
    array[2] |= static_cast<unsigned char>(autonomous_obstacle_crossing);
    // 20
    array[2] &= 0b1111'0111;
    array[2] |= static_cast<unsigned char>(autonomous_trenches);
    // 21
    array[2] &= 0b1111'1011;
    array[2] |= static_cast<unsigned char>(prepare_crossing);
    // 22
    array[2] &= 0b1111'1101;
    array[2] |= static_cast<unsigned char>(rapid_decline);
    // 23
    array[2] &= 0b1111'1110;
    array[2] |= static_cast<unsigned char>(auto_leveling);
    // 24
    array[7] = heart_beat;

    return array;
}

void FrameControl::DecodeFromBytes(const QByteArray &array)
{
    (void)array;
    throw std::runtime_error("Not implemented");
}

QString FrameControl::GetFrameName() const
{
    return "Control";
}
